
#ifndef IMU_ALGORITHM_V2_0_GINS_H
#define IMU_ALGORITHM_V2_0_GINS_H
extern bool check_T_imu(istream& inFile);
extern void swit(int switch_off1[], int& n1);
extern void initial_Matrix(Vector<double, Dynamic>& delta_kBefore,
                    Vector<double, 24>& error_gyro_accel,
                    Matrix<double, Dynamic, Dynamic>& p_kBefore,
                    Matrix<double, Dynamic, Dynamic>& q);
extern Vector3d blh2ned(Vector3d position);
extern void correctGyroAccel(Vector<double, 24> error_gyro_accel,
                      IMUDATA& imuDataNow,
                      IMUDATA imuDataBefore);
extern void  pred(Matrix<double, Dynamic, Dynamic>& p_k_kBefore,
           Vector<double, Dynamic>& delta_k_kBefore,
           Vector<double, Dynamic> delta_kBefore,
           Matrix<double, Dynamic, Dynamic> p_kBefore,
           Matrix<double, Dynamic, Dynamic> q,
           State stateBefore,
           IMUDATA imuDataBefore,
           State stateNow,
           IMUDATA imuDataNow);
extern void MeasUpdate(Matrix<double, Dynamic, Dynamic>& p_k,
                       Vector<double, Dynamic>& delta_k,
                       Matrix<double, Dynamic, Dynamic> p_k_kBefore,
                       Vector<double, Dynamic> delta_k_kBefore,
                       State stateNow,
                       GNSSDATA gnssData);
extern void correctOut(State& stateNow,
                Vector<double, Dynamic>& delta_k);
extern void getErrorGA(Vector<double, Dynamic>& delta_k,
                Vector<double, 24>& error_gyro_accel);
extern Matrix<double, Dynamic, Dynamic> getF(State stateBefore,
                            Quaterniond q_bBefore2nBefore,
                            IMUDATA imuDataBefore,
                            Quaterniond q_b2n,
                            IMUDATA imuDataNow);
extern State interPolate(State stateBefore,
                  State stateNext,
                  GNSSDATA gnssData,
                  double t1,
                  double t2,
                  double t);
extern void initialStateBefore(State& stateBefore,
                        const IMUDATA& imuDataBefore);
#endif //IMU_ALGORITHM_V2_0_GINS_H
